/**RGB-D image*/


#include <iostream>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <eigen3/Eigen/Core>
#include <vector>
#include <boost>     //?

using namespace std;

int main(int argc, char **argv) 
{
    vector<cv::Mat> colorImgs,depthImgs;
    TrajectorType poses;         //typedefine 定义位姿数据类型
    
    ifstream fin("./pose.txt");
    if(!fin)
    {
        cout<<"don't find the pose data"<<endl;
        return -1;
    }
    
    for(int i=0;i<6;i++)
    {
        boost::format fmt("./%s%d.%s");     //定义图像文件格式；
        colorImgs.push_back(cv::imread((fmt %"color" % (i+1) % "png").str()); //"color"+tostring(i+1)+".png"
        depthImgs.push_back(cv::imread(("color"+to_string(i+1)+".png");
        
        double data[7]=0;
        for(auto &d:data)
            fin>>d;
        Sophus::SE3d pose(Eigen::Quaterniond(data[6],data[3],data[4],data[5])); //构建SE3
        Eigen::Vector3d(data[0],data[1],data[2])  //struct t;
        poses.push_back(pose);
    }
    
    double cx=325.5,cy=253.5,fx=518.0,fy=519.0,depthScale=1000.0;
    
    vector<Vector6d,Eigen::aligned_allocator<Vector6d>> pointcloud;  //eigen::aligned_allocator FUNCTION???
    pointcloud.reserve(10000000);  //??
    
    for(int i=0;i<5;i++)
    {
        cout<<"transform image"<<i+1<<endl;
        cv::Mat color=colorImgs[i];
        cv::Mat depth=depthImgs[i];
        Sophus::SE3d T=poses[i];
        for(int v=0;v<color.rows;v++)
        {
            for(int u=0;u<color.cols;u++)
            {
                unsigned int d =depth.ptr<unsigned short>(v)[u];  //深度值
                
                if(d==0) continue;
                Eigen::Vector3d point;
                point[2]=double(d)/depthScale;
                point[0]=(u-cx)*point[2]/fx;
                point[1]=(v-cy)*point[2]/fy;
                Eigen:;Vector3d pointWorld=T*point;
                
                Vector6d p;
                p.head<3>()=pointWorld;
                p[5]=color.data[v*color.step+u*color.channels()];
                p[4]=color.data[v*color.step+u*color.channels()+1];
                p[3]=color.data[v*color.step+u*color.channels()+2];
                pointcloud.push_back(p);
            }
        }
    }
    
    cout<<"share pointcloud"<<pointcloud.size()<<endl;
    showPointCloud(pointcloud);
    return 0;
}
        
                                       
                
                
                
                
                
                
                
                
                
                
                
                
                
                
                
                
                
                
                
                
    
    
    
    
}
